#include "path_base.h"

PlannerBase::PlannerBase(){
    frameId_ = "map";
    startPosePub_ = nh_.advertise<geometry_msgs::PoseStamped>("/gen_start_pose", 1, true);
    goalPosePub_ = nh_.advertise<geometry_msgs::PoseStamped>("/gen_goal_pose", 1, true);
    pathPub_ = nh_.advertise<nav_msgs::Path>("/gen_global_path", 1, true);
    pathOriginPub_ = nh_.advertise<nav_msgs::Path>("/gen_global_path_origin", 1, true);
}

mc::status PlannerBase::genPath(geometry_msgs::Pose start, geometry_msgs::Pose goal, vector<geometry_msgs::Pose>& poses){
    mc::status status;
    poses.clear();
    try{
        ros::Time start_time = ros::Time::now();
        startPosePub_.publish(msg_convert::toPoseStamped(start, frameId_));
        goalPosePub_.publish(msg_convert::toPoseStamped(goal, frameId_));

        // 校验地图数据
        if(!grid_ptr_){
            throw std::runtime_error("grid_ptr_ is nullptr.");
        }

        // 转换起点、终点
        int sx, sy, gx, gy;
        worldToMap(start.position.x, start.position.y, sx, sy);
        worldToMap(goal.position.x, goal.position.y, gx, gy);
        LOG(INFO) << "start: (" << sx << "," << sy << "), goal: (" << gx << "," << gy << ")";
        
        // 校验起点、终点
        if(!valueIsValid(sx, sy) || !valueIsValid(gx, gy)){
            throw std::runtime_error("start or goal is invalid.");
        }

        // 校验路径是否直接生成 
        vector<pair<int, int>> points;
        // 如果起点到终点直线上无障碍物，直接直接生成
        bool line_has_obstacle = false;
        utils::graphis::BresenhamLine(sx, sy, gx, gy, points);
        for(auto& point : points){
            if(!valueIsValid(point.first, point.second)){
                line_has_obstacle = true;
                break;
            }
        }
        if(line_has_obstacle){
            points.clear();
            genGridPath(sx, sy, gx, gy, points);
        }
        // 发布原始路径
        vector<geometry_msgs::Pose> origin_poses;
        for(auto point : points){
            double wx, wy;
            mapToWorld(point.first, point.second, wx, wy);
            geometry_msgs::Pose pose = msg_convert::toPose(wx, wy, 0.0);
            origin_poses.push_back(pose);
        }
        origin_poses = genPathDirection(origin_poses, goal);    // 生成路径方向
        nav_msgs::Path origin_path_msg = msg_convert::toPath(origin_poses, frameId_);
        pathOriginPub_.publish(origin_path_msg);  
        ros::Time path_gen_time = ros::Time::now();

        // 路径平滑
        vector<geometry_msgs::Pose> smooth_poses;
        pathSmooth(origin_poses, smooth_poses);
        poses = genPathDirection(smooth_poses, goal);   // 生成路径方向
        // 发布平滑后路径
        nav_msgs::Path path_msg = msg_convert::toPath(poses, frameId_);
        pathPub_.publish(path_msg);
        ros::Time path_smooth_time = ros::Time::now();

        // 打印耗时
        double gen_time_interval = (path_gen_time - start_time).toSec();
        double smooth_time_interval = (path_smooth_time - path_gen_time).toSec();
        double total_time_interval = (path_smooth_time - start_time).toSec();
        LOG(INFO) << "planning finished. poses size: " << poses.size()
            << ", [time] path_gen: "  << gen_time_interval << "s"
            << ", path_smooth: "  << smooth_time_interval << "s"
            << ", total_cost: "  << total_time_interval << "s";
    }
    catch(const std::exception& e){
        status.success = false;
        status.message = e.what();
        LOG(ERROR) << status.message;
    }
    return status;
}

// 生成路径方向
vector<geometry_msgs::Pose> PlannerBase::genPathDirection(vector<geometry_msgs::Pose>& poses, 
    geometry_msgs::Pose goal){
    vector<geometry_msgs::Pose> new_poses;
    if(poses.size() <= 2){
        new_poses = poses;
        return new_poses;
    }

    for(int i=0; i<poses.size()-1; i++){
        geometry_msgs::Pose curr_pose = poses[i];
        geometry_msgs::Pose next_pose = poses[i+1];
        double angle = utils::math::angleBetweenPoses(curr_pose, next_pose);
        curr_pose.orientation = msg_convert::toGeometryQuaternion(0.0, 0.0, angle);
        new_poses.emplace_back(curr_pose);
    }

    goal.orientation = new_poses.back().orientation;
    new_poses.emplace_back(goal);
    return new_poses;
}

// 平滑路径
void PlannerBase::pathSmooth(vector<geometry_msgs::Pose> origin_poses, vector<geometry_msgs::Pose>& smooth_poses){
    path_smooth_.pathSmooth(SmoothMethod::FIVE_POINT_GRADIENT_DESCENT,
        origin_poses, smooth_poses, 
        std::bind(&PlannerBase::valueIsValidForWorld, this, std::placeholders::_1, std::placeholders::_2));
}


// 数值是否有效
bool PlannerBase::valueIsValid(int x, int y){
    if(grid_ptr_ ){
        if(idxIsVaild(x, y)){
            size_t idx = mapToIndex(x, y);
            return valueIsValidByIdx(idx);
        }
    }
    return false;
}

bool PlannerBase::valueIsValidByIdx(int index){
    if(grid_ptr_ && idxIsVaild(index)){
        // int value = grid_ptr_->data[index];
        // // return  value < 30 && value >= 0;
        // return  value == 0;
        return grid_ptr_->data[index] == 0;
    }
    return false;
}

bool PlannerBase::valueIsValidForWorld(double wx, double wy){
    if(grid_ptr_ ){
        int mx, my;
        worldToMap(wx, wy, mx, my);
        return valueIsValid(mx, my);
    }
    return false;
}